#include <string.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdlib.h>
#include <sys/types.h>
#include <termios.h>
#include <sys/stat.h>

#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>

#include "../include/racecar_driver.h"
#include <ros/ros.h>
#include <ros/package.h>
#include <geometry_msgs/Twist.h>

ros::Publisher vel_pub;
int fd,nByte;

int set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
{
		struct termios newtio,oldtio;
		if  ( tcgetattr( fd,&oldtio)  !=  0) { 
			perror("SetupSerial 1");
			return -1;
		}
		bzero( &newtio, sizeof( newtio ) );
		newtio.c_cflag  |=  CLOCAL | CREAD;
		newtio.c_cflag &= ~CSIZE;
 
		switch( nBits )
		{
			case 7:
				newtio.c_cflag |= CS7;
				break;
			case 8:
				newtio.c_cflag |= CS8;
				break;
		}
 
		switch( nEvent )
		{
		case 'O':
			newtio.c_cflag |= PARENB;
			newtio.c_cflag |= PARODD;
			newtio.c_iflag |= (INPCK | ISTRIP);
			break;
		case 'E': 
			newtio.c_iflag |= (INPCK | ISTRIP);
			newtio.c_cflag |= PARENB;
			newtio.c_cflag &= ~PARODD;
			break;
		case 'N':  
			newtio.c_cflag &= ~PARENB;
			break;
		}
 
		switch( nSpeed )
		{
			case 2400:
				cfsetispeed(&newtio, B2400);
				cfsetospeed(&newtio, B2400);
				break;
			case 4800:
				cfsetispeed(&newtio, B4800);
				cfsetospeed(&newtio, B4800);
				break;
			case 9600:
				cfsetispeed(&newtio, B9600);
				cfsetospeed(&newtio, B9600);
				break;
			case 115200:
				cfsetispeed(&newtio, B115200);
				cfsetospeed(&newtio, B115200);
				break;
			case 460800:
				cfsetispeed(&newtio, B460800);
				cfsetospeed(&newtio, B460800);
				break;
			default:
				cfsetispeed(&newtio, B9600);
				cfsetospeed(&newtio, B9600);
				break;
		}
		if( nStop == 1 )
			newtio.c_cflag &=  ~CSTOPB;
		else if ( nStop == 2 )
			newtio.c_cflag |=  CSTOPB;
			newtio.c_cc[VTIME]  = 0;
			newtio.c_cc[VMIN] = 0;
			tcflush(fd,TCIFLUSH);
		if((tcsetattr(fd,TCSANOW,&newtio))!=0)
		{
			perror("com set error");
			return -1;
		}
		
			printf("串口设置完成!\n\r");
		return 0;
}

void send_cmd(float speed,float angle)
{
    unsigned char sand_buff[14];
    unsigned long sum;

    sand_buff[0] = sand_buff[1] = 0xaa;//帧头
    sand_buff[13] = sand_buff[14] = 0xbb;//帧尾
    sand_buff[2] = ((unsigned short)speed & 0xff00)>> 8;   //取速度的高八位
    sand_buff[3] = (unsigned short)speed & 0x00ff ;   //取速度的低八位
    sand_buff[4] = (unsigned short)(speed * 100) & 0xff;    //取小数点后两位

    sand_buff[5] = (unsigned short)angle & 0x00ff ;   //取速度的低八位
    sand_buff[6] = (unsigned short)(angle * 100) & 0xff;    //取小数点后两位

    sum=0;
    for(int i=2;i<=10;i++)
        sum+=sand_buff[i];
        
    sand_buff[11] = (uint8_t)(sum & 0xff);  //取所有数值(除了帧头帧尾)加起来的低八位

    write(fd,sand_buff,sizeof(sand_buff));
	memset(sand_buff,0,sizeof(sand_buff));
}

void TwistCallback(const geometry_msgs::Twist& twist)
{
    double angle;
    ROS_INFO("x= %f", twist.linear.x);
    ROS_INFO("z= %f", twist.angular.z);
    angle = 2500.0 - twist.angular.z * 2000.0 / 180.0;
    ROS_INFO("angle= %f",double(angle));
    send_cmd((float)twist.linear.x,(float)angle);
}

int main(int argc, char** argv)
{
    char *uart = "/dev/ttyUSB0";

    ros::init(argc, argv, "ultimate_driver");
    ros::NodeHandle n;

    if((fd=open(uart,O_RDWR | O_NOCTTY))<0)
	{
		printf("打开失败\n");
	}else
	{
		set_opt(fd,115200,8,'N',1);
    }

    ros::Subscriber sub = n.subscribe("/car/cmd_vel",1,TwistCallback);

    ros::spin();

}